#include <SoftwareSerial.h>
#include <Servo.h>

Servo LF;
Servo RF;
Servo LB;
Servo RB;
Servo ArmR;
Servo ArmL;
Servo HandR;
Servo HandL;
//SoftwareSerial SS1(3,2);  //RX,TX

String Buffer = "";

void setup() {
  // put your setup code here, to run once:
  InitCar();
}

void loop() {
  //Serial.println(analogRead(A2));
  //SS1.println("hello world");
  /*
  while(SS1.available()>0)
  {
    Buffer += (char)SS1.read();
  }
  Serial.println(Buffer);
  Buffer = "";
  delay(500);
  */
  
  ArmL.write(90);
  ArmR.write(110);
  delay(2000);
}

void InitCar()
{
  LF.attach(7);
  LB.attach(11);
  RF.attach(8);
  RB.attach(12);
  ArmL.attach(6);
  ArmR.attach(5);
  HandR.attach(10);
  HandL.attach(9);
}

void SetArm(int Ang)
{
  ArmR.write(Ang);
  ArmL.write(180-Ang);
}

void SetHand(int Ang)
{
  HandR.write(Ang);
  HandL.write(180-Ang);
}

void Catch()
{
  SetArm(130);
  SetHand(90);  //高举，打开
  delay(500);
  
  SetArm(50);
  delay(300);
  SetHand(130); //放低，抓取
  delay(300);

  SetArm (130); //举起
  delay(300);
}
